#include "Marching.h"
#include <assert.h>
#include <vector>
#include <functional>
#include <numeric>

Marching::Marching(){
	//default
}

Marching::Marching(ImportData* d){

	this->data = d;

	//For 2D, codimension = 1
	/*this->nDim = 2;
	this->nCount = 4;*/
	this->mostconvex = true;
	//For 3D, codimension = 2
	/*this->nDim = 4;
	this->nCount = 5;
	nFilter.resize(this->nDim,this->nCount);*/
	//Define the faces of the cube and the adjacent faces to each face
	face.resize(6); f_adjacent.resize(6); f_opposite.resize(6);
	face[0] = Vector4i(2,10,6,11);/*front*/  f_adjacent[0] = Vector4i(1,2,3,4);			f_opposite[0] = Vector4i(5,-1,-1,-1); 
	face[1] = Vector4i(3,11,7,8); /*left*/   f_adjacent[1] = Vector4i(3,4,5,-1);		f_opposite[1] = Vector4i(2,-1,-1,-1); 
	face[2] = Vector4i(1,9,5,10); /*right*/  f_adjacent[2] = Vector4i(3,4,5,-1);		f_opposite[2] = Vector4i(-1,-1,-1,-1); 
	face[3] = Vector4i(2,1,0,3);  /*bottom*/ f_adjacent[3] = Vector4i(5,-1,-1,-1);		f_opposite[3] = Vector4i(4,-1,-1,-1); 
	face[4] = Vector4i(6,5,4,7);  /*top*/    f_adjacent[4] = Vector4i(5,-1,-1,-1);		f_opposite[4] = Vector4i(-1,-1,-1,-1); 
	face[5] = Vector4i(0,9,4,8);  /*back*/   f_adjacent[5] = Vector4i(-1,-1,-1,-1);		f_opposite[5] = Vector4i(-1,-1,-1,-1); 
	
}

queue<GridElement<Side>> Marching::march3d(){
	cout << "3d march exec." << endl;
	this->nDim = 2;
	this->nCount = 6;
	//Define Indata
	int x,y,z;
	const int elmsize = 12;
	const int sidesize = 6;
	//Plane planeElements[elmsize];// = new Edge[4];
	queue<GridElement<Side>> unchecked_g,checked_g;
	Edge edges[elmsize]; //4 edges per plane
	Side sides[sidesize];
	MatrixXd mat;
	mat.resize(2,3);
	//Create GridElements From the volume
	for(x = 0; x < this->data->getWidth(); ++x){
		for(y = 0; y < this->data->getHeight(); ++y){
			for(z = 0; z < this->data->getDepth(); ++z){
				if((x+1) < (this->data->getWidth()-1) && (y+1) < (this->data->getHeight()-1) && (z+1) < (this->data->getDepth()-1) &&
				   x > 0 && y > 0 && z > 0){ //the point is not at a boundary of the volume
					//Create gridelement from startpoint x,y,z
					//Organize edges to realte to the marching cubes
					//Edges organized according to http://paulbourke.net/geometry/polygonise/
					mat << x,y,z, x+1,y,z; //e1
					edges[2] = Edge(mat);
					mat << x+1,y,z, x+1,y+1,z; //e2
					edges[10] = Edge(mat);
					mat << x,y+1,z, x+1,y+1,z; //e3
					edges[6] = Edge(mat);
					mat << x,y,z, x,y+1,z; //e4
					edges[11] = Edge(mat);

					mat << x,y,z+1, x+1,y,z+1; //e5
					edges[0] = Edge(mat);
					mat << x+1,y,z+1, x+1,y+1,z+1;//e6
					edges[9] = Edge(mat);
					mat << x,y+1,z+1, x+1,y+1,z+1; //e7
					edges[4] = Edge(mat);
					mat << x,y,z+1, x,y+1,z+1; //e8
					edges[8] = Edge(mat);
	
					mat << x+1,y,z, x+1,y,z+1; //e10
					edges[1] = Edge(mat);
					mat << x+1,y+1,z, x+1,y+1,z+1; //e12
					edges[5] = Edge(mat);

					mat << x,y,z, x,y,z+1; //e9
					edges[3] = Edge(mat);
					mat << x,y+1,z, x,y+1,z+1; //e11
					edges[7] = Edge(mat);
					bool empty = false;
					int x1,y1,z1;
					//Check the edges for empty vertices
					for(int j = 0; j < 12; ++j){
						x1 = edges[j].getEndPoints().row(0).x();
						y1 = edges[j].getEndPoints().row(0).y();
						z1 = edges[j].getEndPoints().row(0).z();
						if(this->data->getVolume()[z1][y1][x1].isEmpty()){
							empty = true;
						}
						x1 = edges[j].getEndPoints().row(1).x();
						y1 = edges[j].getEndPoints().row(1).y();
						z1 = edges[j].getEndPoints().row(1).z();
						if(this->data->getVolume()[z1][y1][x1].isEmpty()){
							empty = true;
						}
					}//j
					if(!empty){
						//Add edges to a side and add the sides to the gridelement
						std::vector<Edge> s_edges;
						s_edges.resize(4);
						for(int s = 0;s<6;s++){
							for(int e = 0;e<4;e++){
								s_edges[e] = edges[face[s](e)]; //add edge e of side s to the s_edges
							}
							sides[s] = Side(s_edges); //add the side with the 4 edges defined in s_edges to sides

						}

						GridElement<Side> unCheckedElement(sides,sidesize);
						unchecked_g.push(unCheckedElement);
					} //if !empty
				} //if x+1..
			} // for z
		} //for y
	} //for x
	cout << "starting march" << endl;
	//March
	std::vector<Edge> checkEdges;
	checkEdges.resize(4);
	while(!unchecked_g.empty()){ //Cycles through valid ridgepoints from the queue
		for(int i = 0;i<sidesize;++i){
			checkEdges = unchecked_g.front().getElement(i).getEdges();
			for(int j = 0;j<4;j++){
				if(!checkEdges[j].isChecked()){ //retrieve an edge from the gridelement
					//check the edge
					checkEdges[j] = this->checkEdge3d(checkEdges[j]);
					checkEdges[j].setChecked();
				}
			}//for j
			sides[i] = Side(checkEdges);
		} //for i
		GridElement<Side> checkedElement(sides,sidesize);
		checked_g.push(checkedElement);
		unchecked_g.pop();
	}/* while(!g.empty())*/
	cout << "Done!" << endl;
	return checked_g;
}

Edge Marching::checkEdge3d(Edge &e){
	//cout << "Checking Edge\n" << e.getEndPoints() << endl;
	Edge checkedEdge = e;
	MatrixXd currentEdge;
	Vector3d z,v;
	int h = this->nDim;
	double alpha;
	currentEdge = e.getEndPoints();

	//assert that data is within bounds
	assert(currentEdge.row(0).x() < this->data->getWidth()); 
	assert(currentEdge.row(1).x() < this->data->getWidth());
	assert(currentEdge.row(0).y() < this->data->getHeight());
	assert(currentEdge.row(1).y() < this->data->getHeight());
	assert(currentEdge.row(0).z() < this->data->getDepth());
	assert(currentEdge.row(1).z() < this->data->getDepth());
	//assert(!this->data->getVolume()[z][y][x].isEmpty());

	//Transverse direction
	//compute C
	Matrix3d c;
	c << 0,0,0,
		 0,0,0,
		 0,0,0;
	int k;
	for(k = 0;k<this->nDim;++k){
		v = this->data->getVolume()[(int)currentEdge.row(k).z()][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen();
		c += (v*v.transpose())*1/h;
	}/*for()*/
	//cout << "C: \n" << c << endl;

	//Get the greatest eigenvector from c:
	SelfAdjointEigenSolver<Matrix3d> eigensolver(c);
	if (eigensolver.info() != Success) abort();
		if(eigensolver.eigenvalues()[0] > eigensolver.eigenvalues()[1]){
			if(eigensolver.eigenvalues()[0] > eigensolver.eigenvalues()[2]){
				v = eigensolver.eigenvectors().col(0); 
			}
			else{
				v = eigensolver.eigenvectors().col(2); 
			}
		}
		else{
			if(eigensolver.eigenvalues()[1] > eigensolver.eigenvalues()[2]){
				v = eigensolver.eigenvectors().col(1); 
			}
			else{
				v = eigensolver.eigenvectors().col(2); 
			}
		}
		
	//cout << "average transverse direction: \n " << v << endl;
	//Check the transverse direction of edgepoints vs average, adjust if negative
	double dt;
	Vector3d t;
	for(k = 0;k<this->nDim;++k){
		t = this->data->getVolume()[(int)currentEdge.row(k).z()][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen();
		//cout << "transverse of " << k <<  ": \n" << t << endl;
		dt = v.dot(t);
		if(dt < 0){ //If dot product is negative, multiply transverse with -1 to align it correctly
			//cout << "dot transverse of " << k <<  ": " << dt << endl;
			t *= -1;
			//cout << "adjusted transverse of " << k <<  ": \n" << t << endl;
			this->data->getVolume()[(int)currentEdge.row(k).z()][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].setEigen(t);
		}
	}/*for()*/

	//Zero Crossing
	//Find first derivative at each endpoint
	Vector2d d;
	d << 0,0; //first derivatives at each vertex
	for(k = 0;k<this->nDim;++k){
		Vector3d e,g;
		e = this->data->getVolume()[(int)currentEdge.row(k).z()][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen();
		g = this->data->getVolume()[(int)currentEdge.row(k).z()][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getGradient();
		d(k) = e.dot(g);
		/*cout << "Normalized: " << e << endl;
		cout << "Method : " << this->data->getVolume()[this->slice][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen() << endl;*/
	}/*for()*/
	//cout << "Neighbor: " << i << " First Derivatives: \n" << d << endl;
	//Locate edge that has a positive and negative endpoint and find the zero-crossing
	alpha = 0;
	z << 0,0,0; //zero crossing
	if((d(0) * d(1)) <= 0){
		//Assert that the first derivatives have different signs
		assert((d(0) * d(1)) <= 0);
		//cout << "D0,D1:" << d(0) << "," << d(1) << endl;
		//Interpolate the first derivative to find zero-crossing
		alpha = (d(1) / (d(1) - d(0)));
		//cout << "alpha: " << alpha << ", 1-alpha: " << 1-alpha  << " :: " << (1-alpha)*d(0) + alpha*d(1) << endl;
		//Find zero crossing point
		z = alpha*currentEdge.row(0) + (1-alpha)*currentEdge.row(1);

		//Second Derivatives Check	
		Vector3d e1,e2;
		Matrix3d h1,h2;
		double d2e1,d2e2,d2r;
		e1 = this->data->getVolume()[(int)currentEdge.row(0).z()][(int)currentEdge.row(0).y()][(int)currentEdge.row(0).x()].getEigen();
		e2 = this->data->getVolume()[(int)currentEdge.row(1).z()][(int)currentEdge.row(1).y()][(int)currentEdge.row(1).x()].getEigen();

		h1 = this->data->getVolume()[(int)currentEdge.row(0).z()][(int)currentEdge.row(0).y()][(int)currentEdge.row(0).x()].getHessian();
		h2 = this->data->getVolume()[(int)currentEdge.row(1).z()][(int)currentEdge.row(1).y()][(int)currentEdge.row(1).x()].getHessian();

		d2e1 = (e1.transpose()*h1)*e1;
		d2e2 = (e2.transpose()*h2)*e2;
		//cout << "Second derivatives: " << d2e1 <<  "," << d2e2 << endl;

		//Interpolate to find 2nd derivatives at possible ridge point
		d2r = d2e1*alpha+d2e2*(1-alpha);
		//cout << "Second derivative at ridge point: " << d2r << endl;
		if(d2r < 0){ // the edge has a ridgepoint at the zero crossing!
			//cout << " found ridgepoint!" << endl;
			//Interpolate to find the correct FTLE value at the given point
			double zFTLE,convexity;
			zFTLE = this->data->getVolume()[(int)currentEdge.row(0).z()][(int)currentEdge.row(0).y()][(int)currentEdge.row(0).x()].getFTLE()*alpha + 
				    this->data->getVolume()[(int)currentEdge.row(1).z()][(int)currentEdge.row(1).y()][(int)currentEdge.row(1).x()].getFTLE()*(1-alpha);
			//find the magnitude of the second derivative to describe the convexity at the point
			convexity = d2r * -1;
			//Add information to edge
			checkedEdge.setHasRidgePoint();
			checkedEdge.setRidgePoint(z);
			checkedEdge.setRidgePoint_FTLE(zFTLE);
			checkedEdge.setRidgePoint_Convex(convexity);
		}
	}
	return checkedEdge;
}

queue<GridElement<Side>> Marching::constructSurface(queue<GridElement<Side>> gridelements){
	this->grid_lines3d = queue<GridElement<Side>>(); //empty the line queue
	this->s_lines = queue<GridElement<Side>>(); //empty the line queue
	cout << "Locating lines on faces" << endl;
	this->locateFaceLines(gridelements);
	cout << "Constructing triangles from lines on faces" << endl;
	this->constructTriangles();
	//cout << "Done!" << endl;
	return this->grid_lines3d;
}

//returns the vertex that is different of the line b (third point in the triangle)
int Marching::compareLines(VectorXi a,VectorXi b){
	if(a.x() == b.x()){
		return b.y();
	}
	if(a.y() == b.y()){
		return b.x();
	}
	if(a.x() == b.y()){
		return b.x();
	}
	if(a.y() == b.x()){
		return b.y();
	}
	return -1;
}

void Marching::constructTriangles(){
	Vector3d col1,col2,col3;
	col1 << 1,0,0;
	col2 << 0,1,0;
	col3 << 0,0,1;
	////Go through each gridelement from face_lines and define the appropriate triangle
	this->triCount = 0; //Number of triangles total
	int i,j,k,count,p,s;
	std::vector<VectorXi> start_lines,adjacent_lines,opposite_lines;
	std::vector<int> pos; //edge position in cube and face
	pos.resize(12);
	std::vector<VectorXd> t; //triangle
	t.resize(3);
	std::vector<Edge> edges;//all 12 edges of a cube
	edges.resize(12);
	std::vector<Edge> start_edges; // The 4 edges representing the start side
	std::vector<Edge> adjacent_edges; // The 4 edges representing the adjacent side
	std::vector<Edge> opposite_edges; // The 4 edges representing the opposite side
	start_edges.resize(4);
	adjacent_edges.resize(4);
	opposite_edges.resize(4);
	while(!this->s_lines.empty()){
		//ORGANIZE THE CUBE DATA
		pos.assign(12,0);
		count = 0;
		for(i = 0;i<6;i++){ //Check each side in the gridelement
			start_edges = this->s_lines.front().getElement(i).getEdges();
			for(j = 0;j<4;j++){ //for each edge on the side
				edges[face[i](j)] = start_edges[j];
				if(start_edges[j].hasRidgePoint()){
					pos[face[i](j)] = 1;
				}
			}
		} //for i < 12
		count = accumulate(pos.begin(),pos.end(),0);

		//HANDLE CUBE CASES BASED ON NUMBER OF RIDGEPOINTS FOUND
		if(count == 3){ //if there is exactly 1 triangle in the cube simply define it by its points
			p = 0;
			for(i = 0;i<edges.size();i++){ //Check each edge in the gridelement
				if(edges[i].hasRidgePoint()){
					t[p] = edges[i].getRidgePoint();
					p++;
				}

			} //for i < 12
			this->triCount++;
			this->s_lines.front().addTriangle(t);
			this->s_lines.front().setColor(col3);
		}//ifc=3
		else if(count > 3){ //if there are more than 3 points find the triangle(s) that represents the cube
			//Side order 0:front,1:left,2:right,3:top,4:bottom,5:back
			/*Go through each face -> If current face has lines:
			- check if there are lines in the cube with shared vertices
			- If so connect them
			- Prefer adjacent faces*/
			Side c_side,a_side,o_side; //current side, adjacent side, opposite side
			Vector2i lineA,lineB;
			int compare;
			bool found_adjacent;
			for(s = 0;s<face.size();s++){
				found_adjacent = false;
				c_side = this->s_lines.front().getElement(s); //set the start side
				start_edges = c_side.getEdges();
				if(c_side.hasLines()){//if the front has a line add its points
					start_lines = c_side.getLines();
					for(i = 0;i<start_lines.size();i++){//for each line search for adjacent lines to create a triangle
						lineA = start_lines[i]; //represents a line from the start side
						t[0] = start_edges[lineA.x()].getRidgePoint(); //add line A's ridgepoints
						t[1] = start_edges[lineA.y()].getRidgePoint();
						//check adjacent sides for lines that have same vertices as either t[0] or t[1]
						for(j = 0;j<f_adjacent[s].rows();j++){ //search left,right,top,bottom for a line that connects
							//if the adjacent side is valid and it has line(s)
							if(f_adjacent[s](j) != -1 && this->s_lines.front().getElement(f_adjacent[s](j)).hasLines()){
								a_side = this->s_lines.front().getElement(f_adjacent[s](j));
								adjacent_lines = a_side.getLines();
								//check to see if there is a vertex that is the same as the lineA on any of the adjacent's lines
								for(k = 0;k<adjacent_lines.size();k++){
									lineB =  adjacent_lines[k]; //represents a line from the adjacent side
									compare = compareLines(lineA,lineB);
									if(compare >= 0){//if there is an identical vertex, create a triangle
										//add the ridgepoint from the edge of B that is not identical to form the triangle
										adjacent_edges = a_side.getEdges();
										t[2] = adjacent_edges[compare].getRidgePoint();
										found_adjacent = true;
										//Add triangle to the cube
										this->triCount++;
										this->s_lines.front().addTriangle(t);
										this->s_lines.front().setColor(col1);
										//cout << "adding triangle: " << endl;
										//printVector_eigen(t);
					
									}//if compare
								}//for k

							}//if adjacent
						}//for j
						//check opposite side for lines if no adjacent lines were found //WORK IN PROGRESS
						if(!found_adjacent){
							for(j = 0;j<f_opposite[s].rows();j++){ //search the opposite side for a line
								//if the adjacent side is valid and it has line(s)
								if(f_opposite[s](j) != -1 && this->s_lines.front().getElement(f_opposite[s](j)).hasLines()){
									o_side = this->s_lines.front().getElement(f_opposite[s](j));
									opposite_lines = o_side.getLines();
									//Go through the lines on the opposite side
									for(k = 0;k<opposite_lines.size();k++){
										lineB =  opposite_lines[k]; //represents a line from the opposite side
										//add the two triangles that form from the two opposite lines
										opposite_edges = o_side.getEdges();
										//creates triangle A.x(),A.y(),B.y()
										t[2] = opposite_edges[lineB.y()].getRidgePoint();
										//Add triangle A->B to the cube
										this->triCount++;
										this->s_lines.front().addTriangle(t);
										this->s_lines.front().setColor(col2);
										//creates triangle A.x(),B.x(),B.y()
										t[1] = opposite_edges[lineB.x()].getRidgePoint();
										//Add triangle B->A to the cube
										this->triCount++;
										this->s_lines.front().addTriangle(t);
										this->s_lines.front().setColor(col2);
										//cout << "adding triangle: " << endl;
										//printVector_eigen(t);
									}//for k opposite
								}//if opposite
							}//for j pposite
						}//if !found_adjacent
					}//for i
				}//if c_side
			}//for s

		}//ifc>3
		this->grid_lines3d.push(this->s_lines.front());
		this->s_lines.pop();
	}//while
	cout << "total number of points:\n" << endl;
	//printVector(l);



}

void Marching::locateFaceLines(queue<GridElement<Side>> gridelements){
	int i,j,k,count;
	this->vertCount = 0; //Number of lines total
	Vector2i line1,line2; //A specific line in a gridelement (from x to y)
	std::vector<int> pos,f_pos; //edge position in cube and face
	std::vector<double> c,sorted_c; //convexity of an edgepair
	std::vector<VectorXi> lines;
	const int sidesize = 6;
	Side sides[sidesize];
	pos.resize(12);
	f_pos.resize(4);
	std::vector<Edge> s_edges;
	s_edges.resize(4);

	//Go through each checked gridelement and locate all the correct grid lines
	while(!gridelements.empty()){ 
		//reset the point-table
		pos.assign(12,0);
		count = 0;
		for(i = 0;i<6;i++){ //Check each side in the gridelement
			s_edges = gridelements.front().getElement(i).getEdges();
			for(j = 0;j<4;j++){ //for each edge on the side
				if(s_edges[j].hasRidgePoint()){
					pos[face[i](j)] = 1;
				}
			}
		} //for i < 12
		
		//find resulting count;
		count = accumulate(pos.begin(),pos.end(),count);

		//if the cube has more than 2 ridgepoints, there must be a triangle in this cube
		int f_count;
		if(count > 2){
			/*if(count == 3){
				cout << "Face lines count: " << count << endl;
			}*/
			//check the faces and draw appropriate lines - no crossing lines.
			for(i = 0;i<face.size();i++){
				lines.clear(); //clear lines for each side
				s_edges = gridelements.front().getElement(i).getEdges(); //get the edges for this side
				//find the count on the current face
				f_count = 0;
				f_pos.assign(4,0);
				for(j = 0; j < 4;j++){
					if(pos[face[i](j)] == 1){ //if edge j in face i has a rp increase count.
						f_count++;
						f_pos[j] = 1;
					}
				}//for j

				int e = 0;
				if(f_count == 2){ //if the face has exactly 2 ridgepoints, connect them with a line.
					//cout << "found face with exactly 2 rps." << endl;
					//printVector(f_pos);
					for(j = 0; j < 4;j++){
						if(f_pos[j] == 1){ //this edge has a ridgepoint
							line1(e) = j;//face[i](j);
							e++;
						}
					}
					////Add the line
					vertCount++;
					lines.push_back(line1);
				}//if f_c = 2
				else if(f_count == 3){ //if the face has exactly 3 ridgepoints, connect the two most convex with a line.
					c.clear(); //remove content
					sorted_c.clear(); //remove content

					//find convexity of the lines defined by the 3 ridgepoints
					e = 0;
					Vector3i convexity,edge;
					for(j = 0;j<4;j++){
						if(f_pos[j] == 1){
						edge(e) = j;
						convexity(e) = s_edges[j].getRidgePoint_Convex();
						e++;
						}
					}//for j<4
					//Check convexity between the three edges
					c.resize(3);
					c[0] = convexity(0) * convexity(1); //convexity between edge 0 and 1
					c[1] = convexity(0) * convexity(2); //convexity between edge 0 and 2
					c[2] = convexity(1) * convexity(2); //convexity between edge 1 and 2

					sorted_c = c;
					sort (sorted_c.begin(), sorted_c.end());
					
					for (j = 0;j<c.size();++j){
						if(c[j] == sorted_c[2]){ //if the current value c equals the largest in sorted_c
							//cout << " value: " << i << " is the largest! " << endl;
							switch ( j ) {
								case 0 : 
								line1(0) = edge(0);
								line1(1) = edge(1);
								break;
								case 1 :
								line1(0) = edge(0);
								line1(1) = edge(2);
								break;
								case 2 :
								line1(0) = edge(1);
								line1(1) = edge(2);
								break;
								default : 
								cout << "something went wrong!" << endl;
							}//switch
						}//if
					}//for j>csize
					////Add the most convex line
					//vertCount++;
					//lines.push_back(line1);
				}//if f_c = 3
				else if(f_count == 4){ //if the face has exactly 4 ridgepoints, connect the two separate pairs with a line (pair of most convex, and the other pair).
					//cout << "found face with exactly 4 rps." << endl;
					//printVector(f_pos);
					bool single = false;
					Vector4i convexity;
					for(j = 0;j<4;j++){
						s_edges = gridelements.front().getElement(i).getEdges();
						convexity(j) = s_edges[j].getRidgePoint_Convex();
					}//for j<4
					//Check convexity between the four edges
					c.resize(4);
					sorted_c.resize(4);

					//Convexity of the pattern[\\]
					c[0] = convexity(1) * convexity(2);
					if((convexity(0) * convexity(3)) > c[0]){
						c[0] = convexity(0) * convexity(3);
					}
					//Convexity of the pattern[//]
					c[1] = convexity(2) * convexity(3);
					if((convexity(0) * convexity(1)) > c[1]){
						c[1] = convexity(0) * convexity(1);
					}
					//Convexity of the pattern[|]
					c[2] = convexity(0) * convexity(2);
					//Convexity of the pattern[-]
					c[3] = convexity(1) * convexity(3);

					sorted_c = c;
					sort (sorted_c.begin(), sorted_c.end());

					//find the largest convex
					for (j = 0;j<c.size();++j){
						if(c[j] == sorted_c[3]){ //if the current value c equals the largest in sorted_c
							//cout << " value: " << j << " is the largest! " << endl;
							switch ( j ) {
								case 0 : // = [\\]
								line1(0) = 0;
								line1(1) = 3;
								line2(0) = 1;
								line2(1) = 2;
								break;

								case 1 : // = [//]
								line1(0) = 0;
								line1(1) = 1;
								line2(0) = 2;
								line2(1) = 3;
								break;

								case 2 : // = [|]
								line1(0) = 0;
								line1(1) = 2;
								single = true;
								break;

								case 3 : // = [-]
								line1(0) = 1;
								line1(1) = 3;
								single = true;
								break;

								default : 
								cout << "something went wrong!" << endl;

							}//switch
						}//if
					}//for				
					//Add the lines to the gridelement
					if(single){
						vertCount++;
						lines.push_back(line1);
					}
					else{
						vertCount += 2;
						lines.push_back(line1);
						lines.push_back(line2);
					}
				}//if f_c = 4
				sides[i] = Side(s_edges,lines);
			}//for i
			GridElement<Side> newElement(sides,sidesize);
			this->s_lines.push(newElement);
			this->temp_lines.push(newElement); //temp lines for drawing only lines
		} //if c>2
	gridelements.pop();
	}//While !gridelements
	cout << " Found " << vertCount << " lines" << endl;
}

queue<GridElement<Edge>> Marching::march2d(int s){
	cout << "2d march exec." << endl;
	this->nDim = 2;
	this->nCount = 4;
	//Define Indata
	this->slice = s;
	int x,y;
	const int elmsize = 4;
	Edge edgeElements[elmsize];// = new Edge[4];
	queue<GridElement<Edge>> unchecked_g,checked_g;
	Matrix2d mat;
	mat << 0,0,0,0;

	 //Structure to contain edges
	//Create GridElements From the slice
	for(x = 0; x < this->data->getWidth(); ++x){
		for(y = 0; y < this->data->getHeight(); ++y){
				if((x+1) < this->data->getWidth() && (y+1) < this->data->getHeight()){ //the startpoint is not at the upper or rightmost edge of the slice
					//Create gridelement from startpoint x,y
					Edge e;
					mat << x,y,x+1,y;
					edgeElements[0] = Edge(mat);
					mat << x,y,x,y+1;
					edgeElements[1] = Edge(mat);
					mat << x,y+1,x+1,y+1;
					edgeElements[2] = Edge(mat);
					mat << x+1,y,x+1,y+1;
					edgeElements[3] = Edge(mat);
					
					//check the edges for empty points
					bool empty = false;
					for(int i = 0; i < elmsize; ++i){
						int x1,x2,y1,y2;
						/*cout << "edge: " << edgeElements[i].getEndPoints() << endl;*/
						x1 = edgeElements[i].getEndPoints().row(0).x();
						y1 = edgeElements[i].getEndPoints().row(0).y();
						x2 = edgeElements[i].getEndPoints().row(1).x();
						y2 = edgeElements[i].getEndPoints().row(1).y();
						if(this->data->getVolume()[this->slice][y1][x1].isEmpty() || this->data->getVolume()[this->slice][y2][x2].isEmpty()){
							empty = true;
						}
					} //for i<4
					if(!empty){
						GridElement<Edge> unCheckedElement(edgeElements,elmsize);
						unchecked_g.push(unCheckedElement);
					} //if !empty
				} //if x+1..
		} //for y
	} //for x

	//March
	while(!unchecked_g.empty()){ //Cycles through valid ridgepoints in slice from the queue
		for(int i = 0;i<elmsize;++i){
			if(!unchecked_g.front().getElement(i).isChecked()){
				//cout << "checking: " << g.front().getElement(i).getEndPoints() << endl;

				//ERROR CHECK START
				x = unchecked_g.front().getElement(i).getEndPoints().row(0).x();
				y = unchecked_g.front().getElement(i).getEndPoints().row(0).y();
				assert(!this->data->getVolume()[this->slice][y][x].isEmpty());
				x = unchecked_g.front().getElement(i).getEndPoints().row(1).x();
				y = unchecked_g.front().getElement(i).getEndPoints().row(1).y();
				assert(!this->data->getVolume()[this->slice][y][x].isEmpty());
				//ERROR CHECK END

				edgeElements[i] = this->checkEdge(unchecked_g.front().getElement(i));
				edgeElements[i].setChecked();
			} //if
			else{
				edgeElements[i] = unchecked_g.front().getElement(i);
			}
		} //for
		GridElement<Edge> checkedElement(edgeElements,elmsize);
		checked_g.push(checkedElement);
		unchecked_g.pop();
	}/* while(!g.empty())*/

	return checked_g;
}


Edge Marching::checkEdge(Edge &e){
	//cout << "Checking Edge" << e << endl;
	Edge checkedEdge = e;
	Matrix2d currentEdge,c;
	Vector2d z,v;
	int h = this->nDim;
	double alpha;
	currentEdge = e.getEndPoints();

	//assert that data is within bounds
	assert(currentEdge.row(0).x() < this->data->getWidth()); 
	assert(currentEdge.row(1).x() < this->data->getWidth());
	assert(currentEdge.row(0).y() < this->data->getHeight());
	assert(currentEdge.row(1).y() < this->data->getHeight());

	//Transverse direction
	//compute C
	c << 0,0,0,0;
	int k;
	for(k = 0;k<this->nDim;++k){
		v = this->data->getVolume()[this->slice][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen();
		c += (v*v.transpose())*1/h;
	}/*for()*/
	//cout << "C: \n" << c << endl;

	//Get the greatest eigenvector from c:
	SelfAdjointEigenSolver<Matrix2d> eigensolver(c);
	if (eigensolver.info() != Success) abort();
		if(eigensolver.eigenvalues()[0] > eigensolver.eigenvalues()[1]){
			v = eigensolver.eigenvectors().col(0); 
		}
		else{
			v = eigensolver.eigenvectors().col(1);
		}
		//cout << "average transverse direction: \n " << v << endl;
	//Check the transverse direction of edgepoints vs average, adjust if negative
	double dt;
	Vector2d t;
	for(k = 0;k<this->nDim;++k){
		t = this->data->getVolume()[this->slice][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen();
		//cout << "transverse of " << k <<  ": \n" << t << endl;
		dt = v.dot(t);
		if(dt < 0){ //If dot product is negative, multiply transverse with -1 to align it correctly
			//cout << "dot transverse of " << k <<  ": " << dt << endl;
			t *= -1;
			//cout << "adjusted transverse of " << k <<  ": \n" << t << endl;
			this->data->getVolume()[this->slice][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].setEigen(t);
		}
	}/*for()*/

	//Zero Crossing
	//Find first derivative at each endpoint
	Vector2d d;
	d << 0,0;
	for(k = 0;k<this->nDim;++k){
		Vector2d e,g;
		e = this->data->getVolume()[this->slice][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen();
		g = this->data->getVolume()[this->slice][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getGradient();
		d(k) = e.dot(g);
		/*cout << "Normalized: " << e << endl;
		cout << "Method : " << this->data->getVolume()[this->slice][(int)currentEdge.row(k).y()][(int)currentEdge.row(k).x()].getEigen() << endl;*/
	}/*for()*/
	//cout << "Neighbor: " << i << " First Derivatives: \n" << d << endl;
	//Locate edge that has a positive and negative endpoint and find the zero-crossing
	alpha = 0;
	z << 0,0;
	if((d(0) * d(1)) <= 0){

		//Assert that the first derivatives have different signs
		assert((d(0) * d(1)) <= 0);

		//cout << "D0,D1:" << d(0) << "," << d(1) << endl;
		//Interpolate the first derivative to find zero-crossing
		alpha = (d(1) / (d(1) - d(0)));
		//cout << "alpha: " << alpha << ", 1-alpha: " << 1-alpha  << " :: " << (1-alpha)*d(0) + alpha*d(1) << endl;
		
		//Assert value is 0 at zero-crossing
		//cout << "z-crossing: " << (alpha*d(0) + (1-alpha) * d(1)) << endl;
		//assert((alpha*d(0) + (1-alpha) * d(1)) == 0);

		//Find zero crossing point
		z = alpha*currentEdge.row(0) + (1-alpha)*currentEdge.row(1);

		//Second Derivatives Check	
		Vector2d e1,e2;
		Matrix2d h1,h2;
		double d2e1,d2e2,d2r;
		e1 = this->data->getVolume()[this->slice][(int)currentEdge.row(0).y()][(int)currentEdge.row(0).x()].getEigen();
		e2 = this->data->getVolume()[this->slice][(int)currentEdge.row(1).y()][(int)currentEdge.row(1).x()].getEigen();

		h1 = this->data->getVolume()[this->slice][(int)currentEdge.row(0).y()][(int)currentEdge.row(0).x()].getHessian();
		h2 = this->data->getVolume()[this->slice][(int)currentEdge.row(1).y()][(int)currentEdge.row(1).x()].getHessian();

		d2e1 = (e1.transpose()*h1)*e1;
		d2e2 = (e2.transpose()*h2)*e2;
		//cout << "Second derivatives: " << d2e1 <<  "," << d2e2 << endl;

		//Interpolate to find 2nd derivatives at possible ridge point
		d2r = d2e1*alpha+d2e2*(1-alpha);
		//cout << "Second derivative at ridge point: " << d2r << endl;
		if(d2r < 0){ // the edge has a ridgepoint at the zero crossing!
			//cout << " found ridgepoint!" << endl;
			//Interpolate to find the correct FTLE value at the given point
			double zFTLE,convexity;
			zFTLE = this->data->getVolume()[this->slice][(int)currentEdge.row(0).y()][(int)currentEdge.row(0).x()].getFTLE()*alpha + 
				    this->data->getVolume()[this->slice][(int)currentEdge.row(1).y()][(int)currentEdge.row(1).x()].getFTLE()*(1-alpha);
			//find the magnitude of the second derivative to describe the convexity at the point
			convexity = d2r * -1;

			//Add information to edge
			checkedEdge.setHasRidgePoint();
			checkedEdge.setRidgePoint(z);
			checkedEdge.setRidgePoint_FTLE(zFTLE);
			checkedEdge.setRidgePoint_Convex(convexity);
		}
	}
	return checkedEdge;
}

queue<GridElement<Edge>> Marching::constructRidges(queue<GridElement<Edge>> gridelements){
	cout << "Constructing ridges! " << endl;
	this->grid_lines2d = queue<GridElement<Edge>>(); //empty the line queue
	int i,j,count;
	this->vertCount = 0;
	//colors
	Vector3d col1,col2,col3;
	col1 << 1,0,0;
	col2 << 0,1,0;
	col3 << 0,0,1;
	std::vector<int> points; //Counter for number of points in a given gridelement
	Vector2i line1,line2; //A specific line in a gridelement
	std::vector<double> c;
	std::vector<double> sorted_c;
	//Go through each checked gridelement and draw a line between the two most convex ridgepoints in each gridelement
	while(!gridelements.empty()){ 
		points.assign(4,0); //reset the point-counter
		count = 0;
		for(i = 0;i<4;i++){ //Check each edge in the gridelement
			//cout << "test: " << gridelements.front().getElement(i).getEndPoints() << endl;
			if(gridelements.front().getElement(i).hasRidgePoint()){
				//cout << " has ridgepoint! " << endl;
				//define what edge has the point
				points[i] = 1;
				//update count elements
				count++;
			}
		} //for i < 4
		//cout << "count: " << count << endl;
		if(count == 2){//if there is exactly 2 points we have a single ridgeline
			//mark the line in the gridelement
			//cout << "Gridelement has exactly 2 points: 1 ridgeline." << endl;
			j = 0;
			for(i = 0;i<4;i++){
				if(points[i] == 1){
					line1(j) = i;
					j++;
				}
			}
			//Change color of line
			gridelements.front().setColor(col2);
			//Add line to the gridelement
			this->vertCount++;
			gridelements.front().addLine(line1);
			//add gridelement to ridgeline queue
			this->grid_lines2d.push(gridelements.front());
		}
		else if(count > 2){ //if there are more points we need to evaluate them to find the correct line(s).
			c.clear(); //remove content
			sorted_c.clear(); //remove content
			if(count == 3){ //If count == 3 connect the 2 ridgelines that are most convex between the pairs (1,2),(1,3),(2,3).
					//find convexity of the lines defined by the 3 ridgepoints
					j = 0;
					Vector3i convexity,edge;
					for(i = 0;i<4;i++){
						if(points[i] == 1){
						edge(j) = i;
						convexity(j) = gridelements.front().getElement(edge(j)).getRidgePoint_Convex();
						j++;
						}
					}//for i<4
					//Check convexity between the three edges
					c.resize(3);
					c[0] = convexity(0) * convexity(1); //convexity between edge 0 and 1
					c[1] = convexity(0) * convexity(2); //convexity between edge 0 and 2
					c[2] = convexity(1) * convexity(2); //convexity between edge 1 and 2

					sorted_c = c;
					sort (sorted_c.begin(), sorted_c.end());
					
					for (int i = 0;i<c.size();++i){
						if(c[i] == sorted_c[2]){ //if the current value c equals the largest in sorted_c
							//cout << " value: " << i << " is the largest! " << endl;
							switch ( i ) {
								case 0 : 
								line1(0) = edge(0);
								line1(1) = edge(1);
								break;
								case 1 :
								line1(0) = edge(0);
								line1(1) = edge(2);
								break;
								case 2 :
								line1(0) = edge(1);
								line1(1) = edge(2);
								break;
								default : 
								cout << "something went wrong!" << endl;
							}//switch
						}//if
						if(!this->mostconvex){
							//cout << "checking for second most convex!" << endl;
							if(c[i] == sorted_c[1]){ //if the current value c equals the second largest in sorted_c
							//cout << " value: " << i << " is the largest! " << endl;
							switch ( i ) {
								case 0 : 
								line2(0) = edge(0);
								line2(1) = edge(1);
								break;
								case 1 :
								line2(0) = edge(0);
								line2(1) = edge(2);
								break;
								case 2 :
								line2(0) = edge(1);
								line2(1) = edge(2);
								break;
								default : 
								cout << "something went wrong!" << endl;
							}//switch
							}//if
						}//if
					}//for
					
					////Add the most convex line
					this->vertCount++;
					gridelements.front().addLine(line1);
					if(!this->mostconvex){ //If mostconvex is not true draw the second most convex line as well
						this->vertCount++;
						gridelements.front().addLine(line2);
					}
					gridelements.front().setColor(col3);
					this->grid_lines2d.push(gridelements.front());
				}//if count == 3
				else{ //If count == 4 connect the 2 ridgelines that are most convex.
					//find convexity of the lines defined by the 4 ridgepoints
					j = 0;
					Vector4i convexity,edge;
					for(i = 0;i<4;i++){
						if(points[i] == 1){
						edge(j) = i;
						convexity(j) = gridelements.front().getElement(edge(j)).getRidgePoint_Convex();
						j++;
						}
					}//for i<4
					//Check convexity between the four edges
					c.resize(6);
					sorted_c.resize(6);
					c[0] = convexity(0) * convexity(1); //convexity between edge 0 and 1
					c[1] = convexity(0) * convexity(2); //convexity between edge 0 and 2
					c[2] = convexity(0) * convexity(3); //convexity between edge 0 and 3
					c[3] = convexity(1) * convexity(2); //convexity between edge 1 and 2
					c[4] = convexity(1) * convexity(3); //convexity between edge 1 and 3
					c[5] = convexity(2) * convexity(3); //convexity between edge 2 and 3
					
					sorted_c = c;
					sort (sorted_c.begin(), sorted_c.end());

					//// print out content:
					// cout << "sorted_c contains:";
					// for (int i = 0;i<sorted_c.size();++i){
					//	cout << " " << sorted_c[i];
					// }
					// cout << endl;

					// cout << "c contains:";
					// for (int i = 0;i<c.size();++i){
					//	cout << " " << c[i];
					// }
					// cout << endl;

					//find the largest convex
					for (int i = 0;i<c.size();++i){
						if(c[i] == sorted_c[5]){ //if the current value c equals the largest in sorted_c
							//cout << " value: " << i << " is the largest! " << endl;
							switch ( i ) {
								case 0 : 
								line1(0) = edge(0);
								line1(1) = edge(1);
								break;

								case 1 : 
								line1(0) = edge(0);
								line1(1) = edge(2);
								break;

								case 2 : 
								line1(0) = edge(0);
								line1(1) = edge(3);
								break;

								case 3 : 
								line1(0) = edge(1);
								line1(1) = edge(2);
								break;

								case 4 : 
								line1(0) = edge(1);
								line1(1) = edge(3);
								break;

								case 5 : 
								line1(0) = edge(2);
								line1(1) = edge(3);
								break;

								default : 
								cout << "something went wrong!" << endl;

							}//switch
						}//if
						if(!this->mostconvex){//check for second most convex
							//cout << "checking for second most convex!" << endl;
							if(c[i] == sorted_c[4]){ //if the current value c equals the largest in sorted_c
							//cout << " value: " << i << " is the largest! " << endl;
							switch ( i ) {
								case 0 : 
								line2(0) = edge(0);
								line2(1) = edge(1);
								break;

								case 1 : 
								line2(0) = edge(0);
								line2(1) = edge(2);
								break;

								case 2 : 
								line2(0) = edge(0);
								line2(1) = edge(3);
								break;

								case 3 : 
								line2(0) = edge(1);
								line2(1) = edge(2);
								break;

								case 4 : 
								line2(0) = edge(1);
								line2(1) = edge(3);
								break;

								case 5 : 
								line2(0) = edge(2);
								line2(1) = edge(3);
								break;

								default : 
								cout << "something went wrong!" << endl;

							}//switch
							}//if
						}//if
					}//for				
					//Add the most convex line to the gridelement
					this->vertCount++;
					gridelements.front().addLine(line1);
					if(!this->mostconvex){ //If mostconvex is not true draw the second most convex line as well
						this->vertCount++;
						gridelements.front().addLine(line2);
					}
					gridelements.front().setColor(col1);
					this->grid_lines2d.push(gridelements.front());
			}//else
		}
	gridelements.pop();
	}//While !gridelements

	return this->grid_lines2d;
}

void Marching::printVector_eigen(std::vector<VectorXd> v){
	for (int i = 0;i<v.size();++i){
		cout << " " << v[i] << "\n";
	}
	cout << endl;
}

void Marching::printVector_int(std::vector<int> v){
	for (int i = 0;i<v.size();++i){
		cout << " " << v[i];
	}
	cout << endl;
}

void Marching::setConvex(bool c){
	this->mostconvex = c;
}

queue<GridElement<Edge>> Marching::getRidgeLines2d(){
	return this->grid_lines2d;
}

Marching::~Marching(void){
}
